#ifndef OPTOFORCE_DATA
#define OPTOFORCE_DATA

#include <boost/asio.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/condition.hpp>
#include <string>

#include <curl/curl.h>
#include <tinyxml.h>
#include <boost/lexical_cast.hpp>

class optoforce_data
{
public:
  	optoforce_data(double * list, const std::string &address, unsigned int uSpeed = 100, unsigned int filter = 4);
  	/**
     * @param list double[6], 六维力传感器数据的保存数组，会自动更新，单位N, N, N, Nm, Nm, Nm
     * @param address ip地址
     * @param uSpeed 取样间隔时间，单位ms
     * @param filter
   	**/

  	~optoforce_data();

  	void force_sensor_getData(double * data);
  	/**
     * @brife 单次得到六维力传感器的数据
     * @param data double[6], 六维力传感器数据的保存数组，单位N, N, N, Nm, Nm, Nm
   	**/

	bool waitForNewData();
	/**
     * @brife 判断是否有新的数据返回
     * @return 如果有新数据返回，返回true; 如果等待超时，返回false
   	**/

	bool isRawData() const;	

	void force_sensor_doZero();
	/**
     * @brife 六维力传感器归零
   	**/	
   	
	void force_sensor_doUnzero();
	/**
     * @brife 取消六维力传感器归零
   	**/	
	
protected:
	void recvThreadFunc(void);

	//! Asks EtherDAQ to start streaming data.
	void startStreaming(void);

	enum {DAQ_PORT=49152};
	std::string address_;

	boost::asio::io_service io_service_;
	boost::asio::ip::udp::socket socket_;
	boost::mutex mutex_;
	boost::thread recv_thread_;
	boost::condition condition_;
	volatile bool stop_recv_thread_;
	//! True if recv loop is still running
	bool recv_thread_running_;
	//! Set if recv thread exited because of error
	std::string recv_thread_error_msg_; 

	//! Newest data received from netft device
	//geometry_msgs::WrenchStamped new_data_;
	double new_data_[6] = {0, 0, 0, 0, 0, 0};
	//! Count number of received <good> packets
	unsigned packet_count_;
	//! Count of lost packets using RDT sequence number
	unsigned lost_packets_;
	//! Counts number of out-of-order (or duplicate) received packets
	unsigned out_of_order_count_;
	//! Incremental counter for wrench header
	unsigned seq_counter_;

	//! Speed of EthernetDAQ
	unsigned int speed_;
	unsigned int filter_;

	//! Scaling factor for converting raw force values from device into Newtons
	double force_scale_;
	//! Scaling factor for converting raw torque values into Newton*meters
	double torque_scale_;

	//! Packet count last time diagnostics thread published output
	unsigned diag_packet_count_;
	//! Last time diagnostics was published
	//ros::Time last_diag_pub_time_;

	//! to keep track of out-of-order or duplicate packet
	uint32_t last_hs_sequence_;
	//! to keep track of any error codes reported by netft
	uint32_t system_status_;

	// Units of force
	uint32_t force_units_;
	// Units of torque
	uint32_t torque_units_;

	double * data_list;


	//geometry_msgs::WrenchStamped offset_data_;
	double offset_data_[6] = {0, 0, 0, 0, 0, 0};
	
};


#endif
